package com.hitqz.robot.driver;
import com.google.common.collect.Lists;

import cn.hutool.core.util.StrUtil;
import com.hitqz.robot.api.business.enums.FormConfigEnum;
import com.hitqz.robot.api.business.enums.RobotDriverEnum;
import com.hitqz.robot.api.business.enums.RobotOtherCommandEnum;
import com.hitqz.robot.api.business.enums.RobotParamsFormFieldEnum;
import com.hitqz.robot.api.common.dto.form.FormConfigDto;
import com.hitqz.robot.api.common.dto.form.FormFieldConfigDto;
import com.hitqz.robot.api.common.dto.robot.InitPosDto;
import com.hitqz.robot.api.common.dto.robot.RobotGoTargetPosDto;
import com.hitqz.robot.api.common.dto.robot.RobotMoveDto;
import com.hitqz.robot.api.common.entity.Robot;
import com.hitqz.robot.api.common.entity.RobotMap;
import com.hitqz.robot.api.common.entity.RobotMapPos;
import com.hitqz.robot.common.core.util.R;
import com.hitqz.robot.driver.dto.LocationDto;
import com.hitqz.robot.driver.dto.PoseDto;
import com.hitqz.robot.driver.dto.PowerInfoDto;
import com.hitqz.robot.driver.dto.robot.RobotCustomInfo;
import com.hitqz.robot.driver.model.IRobotPosDto;
import com.hitqz.robot.driver.model.MapAttributesDto;
import com.hitqz.robot.driver.model.PointDto;
import com.hitqz.robot.driver.parent.RobotDriverService;
import com.hitqz.robot.driver.util.JgraphtUtil;
import com.hitqz.robot.driver.util.QueueTypeEnum;
import com.hitqz.robot.driver.util.WheelCacheUtil;
import com.hitqz.robot.driver.util.WheelToolkit;
import lombok.extern.slf4j.Slf4j;
import org.springframework.stereotype.Service;

import java.math.BigDecimal;
import java.math.RoundingMode;
import java.util.*;
import java.util.concurrent.LinkedBlockingDeque;

@Slf4j
@Service("robotDriverService")
public class HitWheelRobotDriverImpl implements RobotDriverService {

    private static double speed = 0.2d;

    private static final double minSpeed = 0.05d;

    private static final double maxSpeed = 1.0d;

    @Override
    public boolean doOnlineMode(Robot robot) {
        boolean result = false;
        if (StrUtil.isNotEmpty(WheelToolkit.serialNumber)) {
            return true;
        }
        try {
            Integer port = null;
            if (robot.getIpAddress().contains(":")) {
                port = Integer.parseInt(robot.getIpAddress().split(":")[1]);
            }
            WheelToolkit.serialNumber = robot.getRobotSn();
            result = WheelToolkit.init(port);
        } catch (Exception e) {
            log.error("[track] doOnlineMode error", e);
        }
        log.info("[wheel] doOnlineMode success");
        return result;
    }


    @Override
    public boolean setSpeed(RobotMoveDto baseSpeedDto) {
        //todo 充电/硬急停触发禁止下发速度 + 发布告警
        //设置标志位,在schedule中监控状态停止速度
        WheelCacheUtil.setSpeedFlag();
        double linerXSpeed = parseAndLimitSpeed(baseSpeedDto.getLinerSpeed(), "0.0", speed, minSpeed, maxSpeed);
        double linerYSpeed = parseAndLimitSpeed(baseSpeedDto.getLinerSpeedY(), "0.0", speed, minSpeed, maxSpeed);
        double angularSpeed = parseAndLimitSpeed(baseSpeedDto.getAngularSpeed(), "0.0", 0.2d, 0.0d, 0.4d);
        WheelToolkit.setSpeed(linerXSpeed, linerYSpeed, 0d, 0d, 0d, angularSpeed);
        return true;
    }

    @Override
    public boolean changeWebControlSpeed(float changeValue) {
        speed = Math.min(Math.max(speed + changeValue, minSpeed), maxSpeed);
        return true;
    }

    @Override
    public boolean setInitPos(InitPosDto baseInitPosDto) {
        log.info("[wheel]下发初始位置 pixel （{},{}）{}", baseInitPosDto.getPosx(), baseInitPosDto.getPosy(), baseInitPosDto.getYaw());
        if (baseInitPosDto.isWebControl()) {
            //web下发的 像素坐标->激光坐标
            if (WheelToolkit.robotMap!=null && !Objects.equals(WheelToolkit.robotMap.getType(),2)){
                BigDecimal bigDec = BigDecimal.valueOf(baseInitPosDto.getYaw());
                float robotYaw = bigDec.setScale(3, RoundingMode.HALF_UP).floatValue();
                baseInitPosDto.setYaw(robotYaw);
                baseInitPosDto.setPosx(baseInitPosDto.getPosx() * WheelToolkit.robotMap.getMapResolution() + WheelToolkit.robotMap.getMapOriginx());
                baseInitPosDto.setPosy((-baseInitPosDto.getPosy()) * WheelToolkit.robotMap.getMapResolution() + WheelToolkit.robotMap.getMapOriginy());
            }
        }
        log.info("[wheel]下发初始位置laserPos （{},{}）{}", baseInitPosDto.getPosx(), baseInitPosDto.getPosy(), baseInitPosDto.getYaw());
        return WheelToolkit.setInitPose(baseInitPosDto.getPosx(), baseInitPosDto.getPosy(), baseInitPosDto.getYaw());
    }

    @Override
    public boolean robotTaskGoTarget(RobotGoTargetPosDto dto) {
        List<RobotMapPos> path = dto.getPath();
        if (path == null || path.isEmpty()) {
            log.warn("[wheel]未找到路径");
            //todo 告警
            return false;
        }
        //按导航类型、拆段字段进行拆分
        WheelToolkit.taskPosQueue = this.splitPath(path);
        WheelToolkit.dealTaskPosQueue();
        return true;
    }

    @Override
    public boolean motionAbort() {
        WheelToolkit.setSpeed(0d, 0d, 0d, 0d, 0d, 0d);
        WheelToolkit.cmdMotion(1);
        return true;
    }

    @Override
    public boolean navCmd(Integer cmd) {
        log.info("[wheel] 收到调度nav cmd:{}", cmd);
        WheelToolkit.cmdMotion(cmd);
        return true;
    }

    @Override
    public boolean cmdCharge(Integer cmd) {
        //取消充电会后退，为了防止撞到东西 ,需判断当前是否在充电
        PowerInfoDto powerInfo = WheelToolkit.getPowerInfo();
        boolean chargeFlag = powerInfo.getChargeStatus() == 4 || powerInfo.getChargeStatus() == 6;
        if (!chargeFlag && Objects.equals(cmd, 0)){
            log.info("[wheel]不在充电，无需下发取消充电");
            return true;
        }
        WheelToolkit.setCharge(cmd);
        return true;
    }


    @Override
    public IRobotPosDto getWebPos() {
        Long heartBeat =(Long) WheelCacheUtil.get(WheelCacheUtil.KEY_HEART_BEAT);
        IRobotPosDto iRobotPosDto = new IRobotPosDto();
        iRobotPosDto.setUrl(Optional.ofNullable(WheelToolkit.robotMap).map(RobotMap::getFilePath).orElse(""));
        iRobotPosDto.setCurrentMap(Optional.ofNullable(WheelToolkit.robotMap).map(RobotMap::getMapCode).orElse(""));
        iRobotPosDto.setWebControlSpeed(BigDecimal.valueOf(speed).divide(BigDecimal.valueOf(100),2, RoundingMode.HALF_UP).floatValue());
        if (heartBeat == null){
            return iRobotPosDto;
        }
        LocationDto locationDto = Optional.ofNullable((LocationDto)WheelCacheUtil.get(WheelCacheUtil.KEY_POSITION_STATUS)).orElse(new LocationDto());
        PowerInfoDto powerInfo = WheelToolkit.getPowerInfo();
        PoseDto currentPose = Optional.ofNullable((PoseDto) WheelCacheUtil.get(WheelCacheUtil.KEY_CURR_POSE)).orElse(new PoseDto());
        iRobotPosDto.setControllerTemp(powerInfo.getTemplate());
        iRobotPosDto.setX(currentPose.getX());
        iRobotPosDto.setY(currentPose.getY());
        iRobotPosDto.setAngle(currentPose.getYaw());
        iRobotPosDto.setCurrentStation(locationDto.getCurrentPosCode());
        //iRobotPosDto.setBlocked(false);
        iRobotPosDto.setBatteryLevel(powerInfo.getPower());
        iRobotPosDto.setCharging(powerInfo.getCurrentDirection()==1);
        //iRobotPosDto.setEmergency(false);
        iRobotPosDto.setCurrentMap(WheelToolkit.getCurrMapCode());
//        iRobotPosDto.setTaskStatus(0);
        iRobotPosDto.setBlockReason("");
        iRobotPosDto.setVoltage(powerInfo.getVoltage());
        iRobotPosDto.setCurrent(String.valueOf(powerInfo.getCurrent()));
        iRobotPosDto.setSpeed(0.0D);
        iRobotPosDto.setPixelX(locationDto.getPixelX());
        iRobotPosDto.setPixelY(locationDto.getPixelY());
        iRobotPosDto.setPixelAngle(locationDto.getPixelAngle());
        iRobotPosDto.setPassedPath(Lists.newArrayList());
//        iRobotPosDto.setTaskStatusStr("");
        iRobotPosDto.setWebControlSpeed(Double.valueOf(speed).floatValue());
        iRobotPosDto.setReached(locationDto.isReach());
        iRobotPosDto.setUrl(WheelToolkit.robotMap==null?null:WheelToolkit.robotMap.getFilePath());
        iRobotPosDto.setCurrentDirection(String.valueOf(powerInfo.getCurrentDirection()));
        iRobotPosDto.setTemperature(powerInfo.getTemplate());
//        todo 二维码坐标
//        iRobotPosDto.setLabel("");
//        iRobotPosDto.setQrCodeX("");
//        iRobotPosDto.setQrCodeY("");
//        iRobotPosDto.setQrCodeZ("");
        return iRobotPosDto;
    }

    @Override
    public List<RobotMapPos> getNavPath(RobotMapPos sourcePos, RobotMapPos targetPos, List<RobotMapPos> allRobotMapPos) {
        //保存信息
        WheelToolkit.posList = allRobotMapPos;
        Queue<Map<Integer, List<RobotMapPos>>> taskPosQueue = new LinkedBlockingDeque<>();
        //路径规划
        List<RobotMapPos> pathPos = new ArrayList<>(16);
        if (Objects.equals(sourcePos.getCode(), targetPos.getCode())) {
            //单点
            pathPos.add(sourcePos);
        } else {
            //多点
            pathPos = JgraphtUtil.getShortestPath(sourcePos.getCode(), targetPos.getCode(), allRobotMapPos);
            PointDto currentPose = (PointDto) WheelCacheUtil.get(WheelCacheUtil.KEY_CURR_POSE);
            if (currentPose != null) {
                double distance = Math.sqrt(Math.pow(currentPose.getX() - sourcePos.getPosX(), 2) + Math.pow(currentPose.getY() - sourcePos.getPosY(), 2));
                if (distance < 0.2d && pathPos.size() > 1) {
                    pathPos.remove(0);
                    log.info("[wheel]当前位置为途径点且和数据库记录位置偏差小,不用下发当前点");
                }
            }
        }
        return pathPos;
    }

    @Override
    public List<RobotMapPos> getRobotMapPos(String mapCode) {
        return List.of();
    }

    @Override
    public List<RobotMap> getMapList() {
        return List.of();
    }

    @Override
    public String getCurrentMap() {
        return WheelToolkit.getCurrMapCode();
    }

    @Override
    public MapAttributesDto getMapAttributes(String mapCode) {
        return null;
    }

    @Override
    public void selMap(RobotMap map) {
        WheelToolkit.robotMap = map;
    }

    @Override
    public PointDto canvas2Point(RobotMap robotMap, PointDto pointDto) {
        if (robotMap!=null) {
            pointDto.setX(pointDto.getX() * robotMap.getMapResolution() + robotMap.getMapOriginx());
            pointDto.setY((-pointDto.getY()) * robotMap.getMapResolution() + robotMap.getMapOriginy());
        }
        return pointDto;
    }

    @Override
    public FormConfigDto getRobotConfig(String formName) {
        // 创建字段配置
        ArrayList<FormFieldConfigDto> fields = new ArrayList<>();
        fields.add(RobotParamsFormFieldEnum.ROBOT_SPEED.toConfigDto());
        fields.add(RobotParamsFormFieldEnum.ULTRASONIC_ENABLE.toConfigDto());
        fields.add(RobotParamsFormFieldEnum.NAV_LIDAR_ENABLED.toConfigDto());
        // 通过 FormConfigEnum 创建表单配置
        return FormConfigEnum.ROBOT_PARAMS_CONFIG.toFormConfigDto(fields);
    }


    @Override
    public R executeOtherCommand(RobotOtherCommandEnum cmdEnum, String jsonParams) {
        boolean result = true;
        switch (cmdEnum) {
            // 超声使能

            // 激光雷达避障使能

            // 下载地图

            // 激光轮廓开关




        }
        return result ? R.ok() : R.failed();
    }

    @Override
    public List<RobotCustomInfo> getCustomInfo() {
        List<RobotCustomInfo> customInfo = new ArrayList<RobotCustomInfo>();
        // 空气质量
        HashMap<String, Double> airMap =(HashMap<String, Double>) WheelCacheUtil.get(WheelCacheUtil.KEY_AIR_QUALITY);
        if (airMap != null) {
            String[] displayKeys = {
                    "smoke", "temperature", "humidity", "ch4", "CO",
                    "O2", "CO2", "aviation_kerosene", "PM2.5", "TVOC",
                    "formaldehyde", "alcohol", "H2S", "NH3", "noise", "PM10"
            };
            String[] displayNames = {
                    "烟雾", "温度", "湿度", "甲烷", "一氧化碳",
                    "氧气", "二氧化碳", "航空煤油", "PM2.5", "挥发性有机物",
                    "甲醛", "酒精", "硫化氢", "氨气", "噪声", "PM10"
            };
            String[] units = {
                    "", "℃", "%", "ppm", "ppm",
                    "%", "%", "ppm", "µg/m³", "ppm",
                    "ppm", "ppm", "ppm", "ppm", "dB", "µg/m³"
            };
            for (int i = 0; i < displayKeys.length; i++) {
                Double value = airMap.get(displayKeys[i]);
                if (value != null) {
                    customInfo.add(RobotCustomInfo.create(displayKeys[i], displayNames[i], value + units[i]));
                }
            }
        }
        return customInfo;
    }

    @Override
    public RobotDriverEnum getDriverVersion() {
        return RobotDriverEnum.HIT;
    }


    /**
     * 解析速度字符串,并限制值
     *
     * @param speedStr     速度字符串
     * @param defaultValue 默认值
     * @return 可以下发的速度
     */
    private double parseAndLimitSpeed(String speedStr, String defaultValue, double controlSpeed, double minControlSpeed, double maxControlSpeed) {
        if (StrUtil.isBlank(speedStr)) {
            return Double.parseDouble(defaultValue);
        }
        double webSpeed = Double.parseDouble(speedStr);
        double limitedAbsSpeed = Math.min(Math.max(Math.abs(webSpeed), minControlSpeed), maxControlSpeed);
        return Double.compare(webSpeed, 0.0) < 0 ? -limitedAbsSpeed : limitedAbsSpeed;
    }

    /**
     * 拆分路径为多个导航队列
     *
     * @param pathPos 路径点
     * @return 导航模式-路径点集合的队列
     */
    private Queue<Map<Integer, List<RobotMapPos>>> splitPath(List<RobotMapPos> pathPos) {
        Queue<Map<Integer, List<RobotMapPos>>> tracePosQueue = new LinkedBlockingDeque<>();
        int navMode = 0;
        List<RobotMapPos> partPathList = new ArrayList<>();
        for (int i = 0; i < pathPos.size(); i++) {
            RobotMapPos pos = pathPos.get(i);
            QueueTypeEnum queueTypeEnum = QueueTypeEnum.MAP.get(pos.getQueueType());
            navMode = switch (queueTypeEnum) {
                case QRCODE -> 5;
                case TRACE -> 4;
                case NAV -> 3;
            };
            if (i < pathPos.size() - 1) {
                boolean splitFlag = pos.getSplitFlag() == 1;
                if (!Objects.equals(pos.getQueueType(), pathPos.get(i + 1).getQueueType())) {
                    splitFlag = true;
                }
                if (!Objects.equals(pos.getMapCode(), pathPos.get(i + 1).getMapCode())) {
                    splitFlag = true;
                }
                partPathList.add(pos);
                if (splitFlag) { // 拆段
                    if (Objects.equals(pos.getQueueType(), QueueTypeEnum.TRACE.getType())
                            && Objects.equals(pathPos.get(i + 1).getQueueType(), QueueTypeEnum.QRCODE.getType())) {
                        // 如果是巡线点到二维码点位，则需把下一个二维码点加入到当前巡线队列中
                        // todo 测试: 以前二维码坐标转换成激光坐标->现在二维码点直接记录激光坐标
                        partPathList.add(pathPos.get(i + 1));
                    }
                    // 记录为一段
                    Map<Integer, List<RobotMapPos>> partMap = new HashMap<>();
                    partMap.put(navMode, partPathList);
                    tracePosQueue.add(partMap);
                    // 重新开始
                    partPathList = new ArrayList<>();
                    if (i >= pathPos.size() - 1) {
                        break;
                    }
                }
            } else if (Objects.equals(pos.getQueueType(), QueueTypeEnum.QRCODE.getType())
                    && navMode == 4) {
                partPathList.add(pos);
            }
        }
        // 记录为一段 从未分段的情况
        if (!partPathList.isEmpty()) {
            Map<Integer, List<RobotMapPos>> partMap = new HashMap<>();
            partMap.put(navMode, partPathList);
            tracePosQueue.add(partMap);
        }
        return tracePosQueue;
    }


}
